/*
 * Matrix3x4.cpp
 *
 *  Created on: 2011/06/24
 *      Author: naoki
 */

#include "NTEngine/NTEngine.h"
#include "NTEngine/Math/Function.h"
#include "NTEngine/Math/Vector3.h"
#include "NTEngine/Math/Quaternion.h"
#include "NTEngine/Math/Matrix3x4.h"

namespace NTEngine {
namespace Math {

Matrix3x4::Matrix3x4( const Matrix3x4& a )
: m11( a.m11 ), m12( a.m12 ), m13( a.m13 ), m14( a.m14 )
, m21( a.m21 ), m22( a.m22 ), m23( a.m23 ), m24( a.m24 )
, m31( a.m31 ), m32( a.m32 ), m33( a.m33 ), m34( a.m34 ) {
}

void Matrix3x4::operator*=( const Matrix3x4& a ) {
	float tx = m11;
	float ty = m12;
	float tz = m13;
	m11 = tx * a.m11 + ty * a.m21 + tz * a.m31;
	m12 = tx * a.m12 + ty * a.m22 + tz * a.m32;
	m13 = tx * a.m13 + ty * a.m32 + tz * a.m33;
	m14 += tx * a.m14 + ty * a.m24 + tz * a.m24;
	tx = m21;
	ty = m22;
	tz = m23;
	m21 = tx * a.m11 + ty * a.m21 + tz * a.m31;
	m22 = tx * a.m12 + ty * a.m22 + tz * a.m32;
	m23 = tx * a.m13 + ty * a.m23 + tz * a.m33;
	m24 += tx * a.m14 + ty * a.m24 + tz * a.m34;
	tx = m31;
	ty = m32;
	tz = m33;
	m31 = tx * a.m11 + ty * a.m21 + tz * a.m31;
	m32 = tx * a.m12 + ty * a.m22 + tz * a.m32;
	m33 = tx * a.m13 + ty * a.m23 + tz * a.m33;
	m34 += tx * a.m14 + ty * a.m24 + tz * a.m34;
}

float Matrix3x4::operator()( int r, int c ) const {
	ASSERT( r >= 0 && r < 3 );
	ASSERT( c >= 0 && c < 4 );
	return ( &m11 )[ 4 * r + c ];
}

float& Matrix3x4::operator()( int r, int c ) {
	ASSERT( r >= 0 && r < 3 );
	ASSERT( c >= 0 && c < 4 );
	return ( &m11 )[ 4 * r + c ];
}

bool Matrix3x4::operator==( const Matrix3x4& m ) const {
	for ( int i = 2; i >= 0; --i ) {
		for ( int k = 3; k >= 0; --k ) {
			if ( operator()( i, k ) != m( i, k ) ) {
				return false;
			}
		}
	}
	return true;
}

bool Matrix3x4::operator!=( const Matrix3x4& m ) const {
	return !( operator==( m ) );
}

void Matrix3x4::setTranslation( float x, float y, float z ) {
	m11 = m22 = m33 = 1.F;
	m14 = x;
	m24 = y;
	m34 = z;
	m12 = m13 = m21 = m23 = m31 = m32 = 0;
}

void Matrix3x4::setTranslation( const Vector3& a ) {
	m11 = m22 = m33 = 1.F;
	m14 = a.x;
	m24 = a.y;
	m34 = a.z;
	m12 = m13 = m21 = m23 = m31 = m32 = 0;
}

void Matrix3x4::setRotationX( float a ) {
	float s = sin( a );
	m22 = m33 = cos( a );
	m23 = -s;
	m32 = s;
	m11 = 1.F;
	m12 = m13 = m21 = m31 = m14 = m24 = m34 = 0;
}

void Matrix3x4::setRotationY( float a ) {
	float s = sin( a );
	m11 = m33 = cos( a );
	m31 = -s;
	m13 = s;
	m22 = 1.F;
	m12 = m21 = m23 = m32 = m14 = m24 = m34 = 0;
}

void Matrix3x4::setRotationZ( float a ) {
	float s = sin( a );
	m11 = m22 = cos( a );
	m12 = -s;
	m21 = s;
	m33 = 1.F;
	m13 = m23 = m31 = m32 = m14 = m24 = m34 = 0;
}

void Matrix3x4::setScaling( float a ) {
	m11 = m22 = m33 = a;
	m12 = m13 = m14 =
	m21 = m23 = m24 =
	m31 = m32 = m34 = 0;
}

void Matrix3x4::setScaling( float x, float y, float z ) {
	m11 = x;
	m22 = y;
	m33 = z;
	m12 = m13 = m14 =
	m21 = m23 = m24 =
	m31 = m32 = m34 = 0;
}

void Matrix3x4::setScaling( const Vector3& a ) {
	m11 = a.x;
	m22 = a.y;
	m33 = a.z;
	m12 = m13 = m14 =
	m21 = m23 = m24 =
	m31 = m32 = m34 = 0;
}

void Matrix3x4::setMul( const Matrix3x4& a, const Matrix3x4& b ) {
	float t11 = a.m11 * b.m11 + a.m12 * b.m21 + a.m13 * b.m31;
	float t12 = a.m11 * b.m12 + a.m12 * b.m22 + a.m13 * b.m32;
	float t13 = a.m11 * b.m13 + a.m12 * b.m23 + a.m13 * b.m33;
	float t14 = a.m11 * b.m14 + a.m12 * b.m24 + a.m13 * b.m34 + a.m14;
	float t21 = a.m21 * b.m11 + a.m22 * b.m21 + a.m23 * b.m31;
	float t22 = a.m21 * b.m12 + a.m22 * b.m22 + a.m23 * b.m32;
	float t23 = a.m21 * b.m13 + a.m22 * b.m23 + a.m23 * b.m33;
	float t24 = a.m21 * b.m14 + a.m22 * b.m24 + a.m23 * b.m34 + a.m24;
	float t31 = a.m31 * b.m11 + a.m32 * b.m21 + a.m33 * b.m31;
	float t32 = a.m31 * b.m12 + a.m32 * b.m22 + a.m33 * b.m32;
	float t33 = a.m31 * b.m13 + a.m32 * b.m23 + a.m33 * b.m33;
	m34 = a.m31 * b.m14 + a.m32 * b.m24 + a.m33 * b.m34 + a.m34;
	m11 = t11;
	m12 = t12;
	m13 = t13;
	m14 = t14;
	m21 = t21;
	m22 = t22;
	m23 = t23;
	m24 = t24;
	m31 = t31;
	m32 = t32;
	m33 = t33;
}

void Matrix3x4::setInverse( const Matrix3x4& m ) {
	float m1122 = m.m11 * m.m22;
	float m1123 = m.m11 * m.m23;
	float m1132 = m.m11 * m.m32;
	float m1133 = m.m11 * m.m33;

	float m1221 = m.m12 * m.m21;
	float m1223 = m.m12 * m.m23;
	float m1231 = m.m12 * m.m31;
	float m1233 = m.m12 * m.m33;

	float m1321 = m.m13 * m.m21;
	float m1322 = m.m13 * m.m22;
	float m1331 = m.m13 * m.m31;
	float m1332 = m.m13 * m.m32;

	float m2132 = m.m21 * m.m32;
	float m2133 = m.m21 * m.m33;

	float m2231 = m.m22 * m.m31;
	float m2233 = m.m22 * m.m33;

	float m2331 = m.m23 * m.m31;
	float m2332 = m.m23 * m.m32;

	float m2233_m2332 = m2233 - m2332;
	float m2331_m2133 = m2331 - m2133;
	float m2132_m2231 = m2132 - m2231;

	float delta = 1.F / ( m.m11 * m2233_m2332 + m.m12 * m2331_m2133 + m.m13 * m2132_m2231 );
	m11 = m2233_m2332 * delta;
	m21 = m2331_m2133 * delta;
	m31 = m2132_m2231 * delta;
	m12 = ( m1332 - m1233 ) * delta;
	m22 = ( m1133 - m1331 ) * delta;
	m32 = ( m1231 - m1132 ) * delta;
	m13 = ( m1223 - m1322 ) * delta;
	m23 = ( m1321 - m1123 ) * delta;
	m33 = ( m1122 - m1221 ) * delta;
	float t14 = m.m14;
	float t24 = m.m24;
	m14 = -( m11 * t14 + m12 * t24 + m13 * m.m34 );
	m24 = -( m21 * t14 + m22 * t24 + m23 * m.m34 );
	m34 = -( m31 * t14 + m32 * t24 + m33 * m.m34 );
}

void Matrix3x4::setTransposed33( const Matrix3x4& a ) {
	m11 = a.m11;
	m12 = a.m21;
	m13 = a.m31;
	m21 = a.m12;
	m22 = a.m22;
	m23 = a.m32;
	m31 = a.m13;
	m32 = a.m23;
	m33 = a.m33;
	m14 = m24 = m34 = 0;
}

void Matrix3x4::setViewTransform( const Vector3& p, const Vector3& t, const Vector3& u ) {
	Vector3 x, y, z;
	z.setSub( p, t );
	z.normalize();
	x.setCross( u, z );
	x.normalize();
	y.setCross( z, x );

	m11 = x.x; m12 = x.y; m13 = x.z;
	m21 = y.x; m22 = y.y; m23 = y.z;
	m31 = z.x; m32 = z.y; m33 = z.z;
	m14 = -( m11 * p.x + m12 * p.y + m13 * p.z );
	m24 = -( m21 * p.x + m22 * p.y + m23 * p.z );
	m34 = -( m31 * p.x + m32 * p.y + m33 * p.z );
}

void Matrix3x4::setRotationQuaternion( const Quaternion& a ) {
	m11 = 1.f - 2.f * a.y * a.y - 2.f * a.z * a.z;
	m21 = 2.f * a.x * a.y + 2.f * a.w * a.z;
	m31 = 2.f * a.x * a.z - 2.f * a.w * a.y;
	m12 = 2.f * a.x * a.y - 2.f * a.w * a.z;
	m22 = 1.f - 2.f * a.x * a.x - 2.f * a.z * a.z;
	m32 = 2.f * a.y * a.z + 2.f * a.w * a.x;
	m13 = 2.f * a.x * a.z + 2.f * a.w * a.y;
	m23 = 2.f * a.y * a.z - 2.f * a.w * a.x;
	m33 = 1.f - 2.f * a.x * a.x - 2.f * a.y * a.y;
	m14 = m24 = m34 = 0;
}

void Matrix3x4::setSlerp( const Matrix3x4& a, const Matrix3x4& b, float t ) {
	Vector3 x( a.m11, a.m21, a.m31 );
	Vector3 y( a.m12, a.m22, a.m32 );
	Vector3 z( a.m13, a.m23, a.m33 );
	Quaternion q1;
	q1.setRotationMatrix( x, y, z );
	x.set( b.m11, b.m21, b.m31 );
	y.set( b.m12, b.m22, b.m32 );
	z.set( b.m13, b.m23, b.m33 );
	Quaternion q2;
	q2.setRotationMatrix( x, y, z );
	Quaternion q;
	q.setSlerp( q1, q2, t );
	setRotationQuaternion( q );
}

void Matrix3x4::identity() {
	m11 = m22 = m33 = 1.F;
	m12 = m13 = m14 =
	m21 = m23 = m24 =
	m31 = m32 = m34 = 0;
}

void Matrix3x4::translate( float x, float y, float z ) {
	m14 += m11 * x + m12 * y + m13 * z;
	m24 += m21 * x + m22 * y + m23 * z;
	m34 += m31 * x + m32 * y + m33 * z;
}

void Matrix3x4::translate( const Vector3& a ) {
	m14 += m11 * a.x + m12 * a.y + m13 * a.z;
	m24 += m21 * a.x + m22 * a.y + m23 * a.z;
	m34 += m31 * a.x + m32 * a.y + m33 * a.z;
}

void Matrix3x4::rotateX( float a ) {
	float c = cos( a );
	float s = sin( a );
	float t;
	t = c * m12 + s * m13;
	m13 = -s * m12 + c * m13;
	m12 = t;
	t = c * m22 + c * m23;
	m23 = -s * m22 + c * m23;
	m22 = t;
	t = c * m32 + s * m33;
	m33 = -s * m32 + c * m33;
	m32 = t;
}

void Matrix3x4::rotateY( float a ) {
	float c = cos( a );
	float s = sin( a );
	float t;
	t = c * m11 - s * m13;
	m13 = s * m11 + c * m13;
	m11 = t;
	t = c * m21 - s * m23;
	m23 = s * m21 + c * m23;
	m21 = t;
	t = c * m31 - s * m33;
	m33 = s * m31 + c * m33;
	m31 = t;
}

void Matrix3x4::rotateZ( float a ) {
	float c = cos( a );
	float s = sin( a );
	float t;
	t = c * m11 + s * m12;
	m12 = -s * m11 + c * m12;
	m11 = t;
	t = c * m21 + s * m22;
	m22 = -s * m21 + c * m22;
	m21 = t;
	t = c * m31 + s * m32;
	m32 = -s * m31 + c * m32;
	m31 = t;
}

void Matrix3x4::scale( float a ) {
	m11 *= a;
	m12 *= a;
	m13 *= a;
	m21 *= a;
	m22 *= a;
	m23 *= a;
	m31 *= a;
	m32 *= a;
	m33 *= a;
}

void Matrix3x4::scale( float x, float y, float z ) {
	m11 *= x;
	m12 *= y;
	m13 *= z;
	m21 *= x;
	m22 *= y;
	m23 *= z;
	m31 *= x;
	m32 *= y;
	m33 *= z;
}

void Matrix3x4::scale( const Vector3& a ) {
	m11 *= a.x;
	m12 *= a.y;
	m13 *= a.z;
	m21 *= a.x;
	m22 *= a.y;
	m23 *= a.z;
	m31 *= a.x;
	m32 *= a.y;
	m33 *= a.z;
}

void Matrix3x4::multiply( const Vector3& in, Vector3* out ) const {
	float tx = in.x;
	float ty = in.y;
	out->x = m11 * tx + m12 * ty + m13 * in.z + m14;
	out->y = m21 * tx + m22 * ty + m23 * in.z + m24;
	out->z = m31 * tx + m32 * ty + m33 * in.z + m34;
}

void Matrix3x4::multiply33( const Vector3& in, Vector3* out ) const {
	float tx = in.x;
	float ty = in.y;
	out->x = m11 * tx + m12 * ty + m13 * in.z;
	out->y = m21 * tx + m22 * ty + m23 * in.z;
	out->z = m31 * tx + m32 * ty + m33 * in.z;
}

void Matrix3x4::invert() {
	setInverse( *this );
}

void Matrix3x4::transpose33() {
	float t; // 入れ替え用
	t = m12; m12 = m21; m21 = t;
	t = m13; m13 = m31; m31 = t;
	t = m23; m23 = m32; m32 = t;
	m14 = m24 = m34 = 0;
}

void Matrix3x4::rotationQuaternion( const Quaternion& q ) {
	Matrix3x4 m;
	m.setRotationQuaternion( q );
	*this *= m;
}

} // namespace Math
} // namespace NTEngine
